#include"obstacle_avoid/obstacle_avoid_fsm.h"

using namespace std;

namespace obstacle_avoid
{
    void ObstacleAvoidFSM::init(ros::NodeHandle &nh)
    {
        current_state = OBS_AVOID_STATE::INIT;
        have_odom = false;
        have_point_cloud = false;
        have_imu = false;
        is_home_set = false;
        is_motion_init = false;
        start_return.data = false;
        is_emergency = false;
        last_return_flag = false;
        imu_data.reset(new sensor_msgs::Imu());
        cloud_data.reset(new pcl::PointCloud<pcl::PointXYZ>);
        last_valid_forward_direction = Eigen::Vector3d::Zero();

        //fsm param
        nh.param("fsm/warning_time_",warning_time, -1.0);
        nh.param("fsm/limit_time_", limit_time, -1.0);
        nh.param("fsm/emergency_time_", emergency_time, -1.0);
        nh.param("fsm/lamda_warning_distance_",lamda_warning_distance, -1.0);
        nh.param("fsm/lamda_limit_distance_", lamda_limit_distance, -1.0);
        nh.param("fsm/lamda_emergency_distance_", lamda_emergency_distance, -1.0);
        nh.param("fsm/max_vel_", max_vel, 1.0);
        nh.param("fsm/max_acc_", max_acc, 1.0);
        nh.param("fsm/hover_vel_threhold_", hover_vel_threhold, 0.01);
        nh.param("fsm/motion_vel_threhold_",motion_vel_threhold,0.1);
        nh.param("fsm/filter_window_size_",filter_window_size_,5);
        nh.param("fsm/emergency_hover_duration_",emergency_hover_duration,5.0);

        //ros timer
        obs_avoid_timer = nh.createTimer(ros::Duration(0.02), &ObstacleAvoidFSM::avoidFSMCallback,this);

        //ros sub
        odom_sub = nh.subscribe("odom",1,&ObstacleAvoidFSM::odomCallback,this);
        cloud_sub = nh.subscribe("scan",1,&ObstacleAvoidFSM::cloudCallback,this);
        imu_sub = nh.subscribe("imu",1,&ObstacleAvoidFSM::imuCallback,this);
        cmd_sub = nh.subscribe("cmd_topic",1,&ObstacleAvoidFSM::cmdCallback,this);
        home_sub = nh.subscribe("home_position",1,&ObstacleAvoidFSM::homeCallback,this);

        //ros pub
        cloud_data_pub = nh.advertise<sensor_msgs::PointCloud2>("filted_cloud_data",10);
        state_pub = nh.advertise<std_msgs::Int16>("obstacle_avoid_state",10);
        return_home_pub = nh.advertise<std_msgs::Bool>("return_home",10);
    }

    
    //利用控制话题中的速度来作为运动方向的判定，避免使用odom的速度（会发生overshoot）
    Eigen::Vector3d ObstacleAvoidFSM::getmotionDirection()
    {
        Eigen::Vector3d motion_direction;
        if(!is_motion_init)
        {
            motion_direction = odom_orient * Eigen::Vector3d::UnitX();
            is_motion_init = true;
        }

        if(cmd_vel.norm() >= motion_vel_threhold)
        {
            motion_direction = cmd_vel.normalized();
            last_valid_forward_direction = motion_direction;
        }
        else if(cmd_vel.norm() <= hover_vel_threhold)
        {
            motion_direction = last_valid_forward_direction;
        }
        else
        {
            Eigen::Vector3d current_velocity_dir = cmd_vel.normalized();
            double angle = acos(last_valid_forward_direction.dot(current_velocity_dir));
            if(angle <= M_PI / 6.0)
            {
                motion_direction = cmd_vel.normalized();
                last_valid_forward_direction = motion_direction;
            }
            else
            {
                motion_direction = last_valid_forward_direction;
            }
        }
        return motion_direction;
    }

    void ObstacleAvoidFSM::downsampleCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data)
    {
        pcl::VoxelGrid<pcl::PointXYZ> voxel;//create voxel filter
        voxel.setInputCloud(cloud_data);
        voxel.setLeafSize(0.1f, 0.1f, 0.1f); // 10cm voxel
        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
        voxel.filter(*filtered);
        cloud_data.swap(filtered);
    }


    void ObstacleAvoidFSM::filterThreatRegion(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data)
    {
        if(cloud_data->empty())
        {
            cout<<"point cloud empty!"<<endl;
            return;
        }

        Eigen::Vector3d forward_direction = getmotionDirection();

        const double radius = 8.0;
        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
        for (const auto& point : cloud_data->points) 
        {
            Eigen::Vector3d relative_pos(point.x, point.y, point.z);
            relative_pos -= odom_pos;
    
            double distance = relative_pos.norm(); // 点到无人机的距离
            if(distance > radius)
            {
                continue;
            }
            // 计算点与运动方向的夹角（余弦值）
            double cos_theta = relative_pos.dot(forward_direction) / distance;
            if(cos_theta < 0) //夹角 > 90°，不在正前方
            {
                continue;
            }
    
            filtered->push_back(point);
        }
        cloud_data.swap(filtered);
    }

    // void ObstacleAvoidFSM::filterThreatRegion(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data)
    // {
    //     if(cloud_data->empty())
    //     {
    //         cout<<"point cloud empty!"<<endl;
    //         return;
    //     }

    //     Eigen::Vector3d forward_direction;
    //     if (odom_vel.norm() < 0.1) 
    //     {
    //         forward_direction = odom_orient * Eigen::Vector3d::UnitX(); // 默认机头方向
    //     }
    //     else
    //     {
    //         forward_direction = odom_vel.normalized();
    //     }
    //     forward_direction.normalize();

    //     const double radius = 3.0;
    //     const double distance = 8.0;

    //     pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
    //     for (const auto& point : cloud_data->points) 
    //     {
    //         Eigen::Vector3d relative_pos(point.x,point.y,point.z);
    //         relative_pos -= odom_pos;
    //         double projection = relative_pos.dot(forward_direction);

    //         //只判定运动方向前方的，只筛选运动朝向前方的点云
    //         if(projection > distance || projection < 0)
    //         {
    //             continue;
    //         }
    //         //运动方向前后都做判定
    //         // if(abs(projection) > distance)
    //         // {
    //         //     continue;
    //         // }
    //         Eigen::Vector3d lateral_vec = relative_pos - projection * forward_direction;
    //         double lateral_distance = lateral_vec.norm();
    //         if(lateral_distance < radius)
    //         {
    //             filtered->push_back(point);
    //         }
    //     }
    //     cloud_data.swap(filtered);
    // }

    void ObstacleAvoidFSM::preprocessPointCloud(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data)
    {
        downsampleCloud(cloud_data);
        filterThreatRegion(cloud_data);
    }

    void ObstacleAvoidFSM::nearestDistance(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data) 
    {
        if (cloud_data->empty() || !have_point_cloud) 
        {
            ROS_WARN_ONCE("Point cloud is empty! Threat region filtering skipped.");
            obs_dis = std::numeric_limits<double>::infinity();
            obstaclePos.setZero();
            return;
        }
    
        if (odom_vel.norm() < 1e-6)
        {
            obs_dis = std::numeric_limits<double>::infinity();
            return;
        }
    
        // 定义运动方向
        Eigen::Vector3d forward_direction = getmotionDirection();
    
        // 设置检测角度范围（正前方45°内）
        const double max_angle = M_PI / 4.0; // 45°
        const double cos_threshold = cos(max_angle);
    
        //无人机半径（用于膨胀）
        const double uav_radius = 0.3; 
        pcl::PointCloud<pcl::PointXYZ>::Ptr filted_cloud(new pcl::PointCloud<pcl::PointXYZ>());
        for(const auto &point : cloud_data->points)
        {
            //计算相对位置
            Eigen::Vector3d relative_pos(point.x, point.y, point.z);
            relative_pos -= odom_pos;

            double distance = relative_pos.norm();
            if(distance < 1e-4)
            {
                continue;
            }

            double cos_theta = relative_pos.dot(forward_direction) / distance;
            if(cos_theta >= cos_threshold)
            {
                filted_cloud->points.push_back(point);
            }
        }

        if(filted_cloud->empty())
        {
            obs_dis = std::numeric_limits<double>::infinity();
            obstaclePos.setZero();
            temporalFilter();
            return;
        }


        pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
        kdtree.setInputCloud(filted_cloud);
        //设置参数
        const int k = 1;
        std::vector<int> point_index(k);
        std::vector<float> point_distance(k);

        pcl::PointXYZ searchPoint;
        searchPoint.x = odom_pos.x();
        searchPoint.y = odom_pos.y();
        searchPoint.z = odom_pos.z();
        
        if(kdtree.nearestKSearch(searchPoint,k,point_index,point_distance) > 0)
        {
            pcl::PointXYZ nearest_point = filted_cloud->points[point_index[0]];
            obstaclePos(0) = nearest_point.x;
            obstaclePos(1) = nearest_point.y;
            obstaclePos(2) = nearest_point.z;

            double distance = sqrt(point_distance[0]);
            obs_dis = distance - uav_radius;

            if(obs_dis < 0.0)
            {
                obs_dis = 0.0;
            }
        }
        else
        {
            obs_dis = std::numeric_limits<double>::infinity();
            obstaclePos.setZero();
        }
    
        // // 初始化最近障碍物信息
        // double nearest_distance = std::numeric_limits<double>::infinity();
        // Eigen::Vector3d nearest_point;
    
        // for (const auto& point : cloud_data->points) 
        // {
        //     Eigen::Vector3d relative_pos(point.x, point.y, point.z);
        //     relative_pos -= odom_pos;
    
        //     double distance = relative_pos.norm();
        //     if (distance > nearest_distance) continue; // 提前剪枝
    
        //     // 计算点与运动方向的夹角（余弦值）
        //     double cos_theta = relative_pos.dot(forward_direction) / distance;
        //     // 不在正前方范围内
        //     if (cos_theta < cos_threshold) 
        //     {
        //         continue; 
        //     }
    
        //     // 更新最近点
        //     nearest_distance = distance;
        //     nearest_point = relative_pos + odom_pos;
        // }
    
        // //添加无人机半径的膨胀
        // obs_dis = nearest_distance - uav_radius;
        // obstaclePos = nearest_point;
    
        // //确保距离不为负数（避免误判）
        // if(obs_dis < 0.0) 
        // {
        //     obs_dis = 0.0;
        // }
            
    
        // 调用时间窗口滤波
        temporalFilter();
    }

    // void ObstacleAvoidFSM::nearestDistance(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data) 
    // {
    //     if (cloud_data->empty() || !have_point_cloud) 
    //     {
    //         ROS_WARN_ONCE("Point cloud is empty! Threat region filtering skipped.");
    //         obs_dis = std::numeric_limits<double>::infinity();
    //         obstaclePos.setZero();
    //         return;
    //     }
    //     if(odom_vel.norm() < 1e-6)
    //     {
    //         obs_dis = std::numeric_limits<double>::infinity();
    //         return;
    //     }

    //     //对搜索的障碍物距离添加上运动的约束
    //     Eigen::Vector3d forward_direction;
    //     if (odom_vel.norm() < 0.1) 
    //     {
    //         forward_direction = odom_orient * Eigen::Vector3d::UnitX(); // 默认机头方向
    //     }
    //     else
    //     {
    //         forward_direction = odom_vel.normalized();
    //     }
    //     forward_direction.normalize();
    //     //向前筛选的参数
    //     const double max_angle = M_PI / 4.0; //90度的搜索角度
    //     const double cos_threshold = cos(max_angle);

    //     pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
    //     kdtree.setInputCloud(cloud_data);

    //     pcl::PointXYZ query_point;
    //     query_point.x = odom_pos(0);
    //     query_point.y = odom_pos(1);
    //     query_point.z = odom_pos(2);

    //     // 直接搜索最近单点
    //     std::vector<int> indices(1);
    //     std::vector<float> distances(1);
    //     if (kdtree.nearestKSearch(query_point, 1, indices, distances) > 0) 
    //     {
    //         // 直接使用最近点
    //         pcl::PointXYZ pt = cloud_data->points[indices[0]];
    //         obstaclePos = Eigen::Vector3d(pt.x, pt.y, pt.z);
    //         obs_dis = (obstaclePos - odom_pos).norm();
    //     } 
    //     else 
    //     {
    //         obs_dis = std::numeric_limits<double>::infinity();
    //     }

    //     // 调用时间窗口滤波
    //     temporalFilter();
    // }

    void ObstacleAvoidFSM::temporalFilter()
    {
        if (distance_history_.size() >= filter_window_size_) 
        {
            distance_history_.pop_front();
        }
        distance_history_.push_back(obs_dis);

        // 计算滑动平均
        double sum = 0.0;
        for (const auto &d : distance_history_) 
        {
            sum += d;
        }
        obs_dis = sum / distance_history_.size();
    }

    bool ObstacleAvoidFSM::isHovering()
    {
        const double vel_threshold = 0.05;
        const double ang_threshold = 0.05;
        bool isVelHover = (odom_vel.norm() < vel_threshold);
        bool isAngHover = (odom_angular.norm() < ang_threshold);
        return (isVelHover && isAngHover);
    }

    // void ObstacleAvoidFSM::calculateT2C()
    // {
    //     double v_current = odom_vel.norm();
    //     if(v_current < 1e-6)
    //     {
    //         t2c = std::numeric_limits<double>::infinity(); 
    //     }
    //     double dmin = calculateDynamicDmin(v_current,max_acc);
    //     double dobs = nearestDistance(cloud_data);
    //     Eigen::Vector3d deltaPos = obstaclePos - odom_pos;
    //     if(dobs < 1e-6)
    //     {
    //         t2c = 0.0;
    //     }

    //     double v_proj = odom_vel.dot(deltaPos)/deltaPos.norm();
    //     if(v_proj <= 0) //速度方向与障碍物方向夹角大于90度
    //     {
    //         t2c = std::numeric_limits<double>::infinity(); 
    //     }

    //     if(dobs <= dmin)
    //     {
    //         t2c = emergency_time;
    //     }
    //     else
    //     {
    //         t2c = (dobs - dmin) / v_proj;
    //     }

    // }

    void ObstacleAvoidFSM::calculateDis()
    {
        double v_current = odom_vel.norm();
        double dmin = calculateDynamicDmin(v_current, max_acc);
        d_waring = max(2.5,lamda_warning_distance * dmin);
        d_limit = max(1.5,lamda_limit_distance * dmin);
        d_emergency = max(0.8,lamda_emergency_distance * dmin);
    }

    //test flited point cloud
    void ObstacleAvoidFSM::cloudPub(pcl::PointCloud<pcl::PointXYZ>::Ptr &cloud_data)
    {
        sensor_msgs::PointCloud2 output;
        pcl::toROSMsg(*cloud_data,output);
        output.header.frame_id="map";
        cloud_data_pub.publish(output);
    }

    void ObstacleAvoidFSM::statePub()
    {
        std_msgs::Int16 msg;
        msg.data = getCurrentState();
        state_pub.publish(msg);
    }

    void ObstacleAvoidFSM::homeCallback(const geometry_msgs::PoseStampedConstPtr &msg)
    {
        home_pos(0) = msg->pose.position.x;
        home_pos(1) = msg->pose.position.y;
        home_pos(2) = msg->pose.position.z;

        is_home_set = true;
    }

    void ObstacleAvoidFSM::odomCallback(const nav_msgs::OdometryConstPtr &msg)
    {
        odom_pos(0) = msg->pose.pose.position.x;
        odom_pos(1) = msg->pose.pose.position.y;
        odom_pos(2) = msg->pose.pose.position.z;

        odom_vel(0) = msg->twist.twist.linear.x;
        odom_vel(1) = msg->twist.twist.linear.y;
        odom_vel(2) = msg->twist.twist.linear.z;

        odom_angular(0) = msg->twist.twist.angular.x;
        odom_angular(1) = msg->twist.twist.angular.y;
        odom_angular(2) = msg->twist.twist.angular.z;

        odom_orient.w() = msg->pose.pose.orientation.w;
        odom_orient.x() = msg->pose.pose.orientation.x;
        odom_orient.y() = msg->pose.pose.orientation.y;
        odom_orient.z() = msg->pose.pose.orientation.z;

        have_odom = true;
    }

    //传感器的点云数据需要进行处理，需要从雷达坐标系下转化到惯性坐标系，先从雷达坐标系转化到机体坐标系，再从机体坐标系转化到惯性坐标系
    void ObstacleAvoidFSM::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &msg)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr temp_cloud(new pcl::PointCloud<pcl::PointXYZ>);
        pcl::fromROSMsg(*msg, *temp_cloud);
        
        //body系下的点云数据
        pcl::PointCloud<pcl::PointXYZ> latest_cloud_b;
        //惯性系下的点云数据
        pcl::PointCloud<pcl::PointXYZ> latest_cloud_w;
        //Eigen::Quaterniond q(imu_data->orientation.w,imu_data->orientation.x,imu_data->orientation.y,imu_data->orientation.z);
        double x = odom_pos(0);
        double y = odom_pos(1);
        double z = odom_pos(2);
        double roll;
        double pitch;
        double yaw;
        /* ROS中的四元数默认的顺序是: (x, y, z, w); odom默认顺序(w,x,y,z)*/
        geometry_msgs::Quaternion odom_orientation;
        odom_orientation.w = odom_orient.w();
        odom_orientation.x = odom_orient.x();
        odom_orientation.y = odom_orient.y();
        odom_orientation.z = odom_orient.z();
        tf::Quaternion q;
        tf::quaternionMsgToTF(odom_orientation,q);
        tf::Matrix3x3(q).getRPY(roll,pitch,yaw);
        //lidar to body
        //雷达向前倾斜15度
        //pcl::transformPointCloud(*temp_cloud,latest_cloud_b,pcl::getTransformation(0,0,0.1,0.0,0.2618,0.0));
        //雷达正常摆放
        pcl::transformPointCloud(*temp_cloud,latest_cloud_b,pcl::getTransformation(0,0,0.05,0.0,0.0,0.0));
        //Eigen::Quaterniond q = odom_orient;
        pcl::transformPointCloud(latest_cloud_b,latest_cloud_w,pcl::getTransformation(x,y,z,roll,pitch,yaw));

        //滤除无人机半径周围的点
        const double uav_radius = 0.5;  
        pcl::PointCloud<pcl::PointXYZ>::Ptr filtered(new pcl::PointCloud<pcl::PointXYZ>);
        for (const auto& point : latest_cloud_w.points) 
        {
            Eigen::Vector3d relative_pos(point.x, point.y, point.z);
            relative_pos -= odom_pos;

            double distance = relative_pos.norm();
            if(distance < uav_radius ) 
            {
                continue;
            }
            filtered->push_back(point);
        }

        *cloud_data = *filtered;
    
        have_point_cloud = true;
    }

    void ObstacleAvoidFSM::imuCallback(const sensor_msgs::ImuConstPtr &msg)
    {
        *imu_data = *msg;
        have_imu = true;
    }

    void ObstacleAvoidFSM::cmdCallback(const mavros_msgs::PositionTargetConstPtr &msg)
    {
        cmd_vel(0) = msg->velocity.x;
        cmd_vel(1) = msg->velocity.y;
        cmd_vel(2) = msg->velocity.z;
    }

    void ObstacleAvoidFSM::changeFSMState(OBS_AVOID_STATE new_state, string pos_call)
    {
        static string state_str[7] = {"INIT","MANUAL_FLIGHT","WARNING_ALERT","OPERATOR_LIMIT","EMERGENCY_HOVER","RETURN_HOME"};
        int pre_s = int(current_state);
        current_state = new_state;
        cout<<"Change FSM State:"<<endl;
        cout<<"["+pos_call+"] : from " + state_str[pre_s] + " to " + state_str[int(new_state)]<<endl;
    }

    void ObstacleAvoidFSM::printFSMState()
    {
        static string state_str[7] = {"INIT","MANUAL_FLIGHT","WARNING_ALERT","OPERATOR_LIMIT","EMERGENCY_HOVER","RETURN_HOME"};
        cout << "[FSM]: state: " + state_str[int(current_state)] << endl;
    }

    void ObstacleAvoidFSM::avoidFSMCallback(const ros::TimerEvent &e)
    {
        obs_avoid_timer.stop(); //to avoid blockage

        //preprocess point cloud data
        preprocessPointCloud(cloud_data);
        //calculate nearest point distance
        nearestDistance(cloud_data);

        //get distance for state change
        calculateDis();

        cloudPub(cloud_data);

        statePub();
        
        static int fsm_num = 0;
        fsm_num++;
        //print current_state per second
        if(fsm_num == 50)
        {
            printFSMState();
            if(!have_odom)
            {
                cout<<"no odom!"<<endl;
            }
            else if(!have_point_cloud)
            {   
                cout<<"no point cloud!"<<endl;
            }
            //debug test
            //cout<<"the forward direction is "<<odom_orient * Eigen::Vector3d::UnitX()<<endl;
            //cout<<this->t2c<<endl;
            cout<<"obstalce distance is "<<this->obs_dis<<endl;
            //cout<<this->d_emergency<<endl;
            cout<<"current velocity is "<<this->odom_vel.x()<<", "<<this->odom_vel.y()<<", "<<this->odom_vel.z()<<endl;
            //cout<<this->max_acc<<endl;
            cout<<"last motion direction: "<<last_valid_forward_direction.x()<<", "<<last_valid_forward_direction.y()<<", "<<last_valid_forward_direction.z()<<endl;
            fsm_num = 0;
        }

        // if(isHovering())
        // {
        //     //obs_dis = std::numeric_limits<double>::infinity();
        //     if (current_state != MANUAL_FLIGHT && current_state != EMERGENCY_HOVER) 
        //     {
        //         changeFSMState(MANUAL_FLIGHT, "Hovering");
        //     }
        //     goto force_return;
        // }

        switch(current_state)
        {
        case INIT:
        {
            if(!have_odom)
            {
                goto force_return;
            }

            changeFSMState(MANUAL_FLIGHT, "FSM");
            break;
        }
        
        case MANUAL_FLIGHT:
        {
            if(obs_dis <= d_waring && obs_dis > d_limit)
            {
                changeFSMState(WARNING_ALERT,"FSM");
            }
            break;
        }

        case WARNING_ALERT:
        {
            if(obs_dis <= d_limit&& obs_dis > d_emergency)
            {
                changeFSMState(OPERATOR_LIMIT, "FSM");
            }
            else if(obs_dis > d_waring)
            {
                changeFSMState(MANUAL_FLIGHT, "FSM");
            }
            break;
        }

        case OPERATOR_LIMIT:
        {
            if(obs_dis <= d_emergency)
            {
                changeFSMState(EMERGENCY_HOVER, "FSM");
            }
            else if(obs_dis > d_limit)
            {
                changeFSMState(WARNING_ALERT, "FSM");
            }
            break;
        }
        
        case EMERGENCY_HOVER:
        {
            if(!is_emergency)
            {
                emergency_start_time = ros::Time::now();
                is_emergency = true;
            }
            double duration = (ros::Time::now() - emergency_start_time).toSec();
            
            // if((ros::Time::now() - emergency_start_time).toSec() > emergency_hover_duration && is_home_set)
            if(duration > emergency_hover_duration && is_home_set)
            {
                changeFSMState(RETURN_HOME, "FSM");
                is_emergency = false;
            }
            break;
        }

        case RETURN_HOME:
        {
            bool reach_home = ((odom_pos - home_pos).norm() < 0.5);
            
            if(reach_home)
            {
                changeFSMState(MANUAL_FLIGHT,"FSM");
                start_return.data = false;
            }
            else
            {
                start_return.data = true;
            }
            if(last_return_flag != start_return.data)
            {
                return_home_pub.publish(start_return);
                last_return_flag = start_return.data;
            }
            
        }
        }

    force_return:;
        obs_avoid_timer.start();

    }
} //namespace obstacle_avoid
